/*
 * robot.cpp
 *
 *  Created on: Jun 25, 2009
 *      Author: matt
 */

#include "robot.h"

void ROBOT::GetHull(vector<LINE2DF>& rhull) {
	for (unsigned int i = 0; i < hull.size(); i++) {
		unsigned int index = 2 * i;
		VECTOR2DF r1 = { xpos - hull_distances[index] * sin(rrot
				- hull_offsets[index]), ypos + hull_distances[index] * cos(rrot
				- hull_offsets[index]) };
		index = 2 * i + 1;
		VECTOR2DF r2 = { xpos - hull_distances[index] * sin(rrot
				- hull_offsets[index]), ypos + hull_distances[index] * cos(rrot
				- hull_offsets[index]) };
		rhull.push_back(LINE2DF(r1, r2));
	}

}

void ROBOT::SetWheels(VECTOR2DF direction1, float dist1, VECTOR2DF direction2,
		float dist2) {
	wheel_directions[0] = direction1;
	wheel_directions[1] = direction2;
	wheel_distances[0] = dist1;
	wheel_distances[1] = dist2;
}

void ROBOT::GetWheels(VECTOR2DF &first, VECTOR2DF &second) {
	VECTOR2DF unit = { 0, 1 };
	float o1, o2;
	float fx1 = wheel_directions[0].x < 0 ? -1 : 1;
	o1 = fx1 * angle(unit, wheel_directions[0]);
	float fx2 = wheel_directions[1].x < 0 ? -1 : 1;
	o2 = fx2 * angle(unit, wheel_directions[1]);
	first.x = xpos - wheel_distances[0] * sin(rrot - o1);
	first.y = ypos + wheel_distances[0] * cos(rrot - o1);
	second.x = xpos - wheel_distances[1] * sin(rrot - o2);
	second.y = ypos + wheel_distances[1] * cos(rrot - o2);

}

void ROBOT::GetCenterFromFirstWheel(VECTOR2DF & center, VECTOR2DF wheel) {
	VECTOR2DF unit = { 0, 1 };
	float o1;
	float fx1 = wheel_directions[0].x < 0 ? -1 : 1;
	o1 = fx1 * angle(unit, wheel_directions[0]);
	center.x = wheel.x + wheel_distances[0] * sin(rrot - o1);
	center.y = wheel.y - wheel_distances[0] * cos(rrot - o1);
}

void ROBOT::Init() {
	laser_on = false;
	rrot = 0;
	xpos = 0;
	ypos = 0;
	axle_width = 1.0f;
	speed[0] = 0;
	speed[1] = 0;

	//here we initialize the hull
	VECTOR2DF unit = { 0, 1 }, origin = { 0, 0 }, e1, e2;

	for (unsigned int i = 0; i < hull.size(); i++) {
		endpoints(hull[i], e1, e2);
		float fx1 = e1.x < 0 ? -1 : 1;
		float fx2 = e2.x < 0 ? -1 : 1;
		hull_offsets.push_back(fx1 * angle(unit, e1));
		hull_offsets.push_back(fx2 * angle(unit, e2));
		hull_distances.push_back(dist(origin, e1));
		hull_distances.push_back(dist(origin, e2));
	}
}

void ROBOT::Rotate(float angle) {
	rrot += angle;
}

void ROBOT::Translate(float x, float y) {
	xpos += x;
	ypos += y;
}

void ROBOT::SetResetPoint() {
	reset_pnt[0] = xpos;
	reset_pnt[1] = ypos;
	reset_pnt[2] = rrot;
}

void ROBOT::Reset() {
	xpos = reset_pnt[0];
	ypos = reset_pnt[1];
	rrot = reset_pnt[2];
}

void ROBOT::SyncRobot(float sync[]) {
	xpos = sync[0];
	ypos = sync[1];
	rrot = sync[2];
}

void ROBOT::GetLaser(vector<LINE2DF> & l) {
	l = laser;
}

bool ROBOT::LaserOn() {
	return laser_on;
}

void ROBOT::SetLaserOn(bool on) {
	laser_on = on;
}

void ROBOT::Laser() {

	laser.clear();
#define STEPSIZE M_PI/10;
	float f = STEPSIZE;
	float upperbound = M_PI - STEPSIZE;
	while (f < upperbound) {
		VECTOR2DF slope = { cos(f), sin(f) };
		VECTOR2DF origin = { xpos, ypos };
		float length = 100;
		laser.push_back(LINE2DF(slope, origin, length));
		f += STEPSIZE;
	}
}

